
#ifndef CAROS_KUKA_ROBOTS_H
#define CAROS_KUKA_ROBOTS_H

#include <caros/caros_node_service_interface.h>
#include <caros/kuka_lwr.h>
#include <caros/kuka_lwr_msgs.h>
#include <caros/serial_device_service_interface.h>
#include <caros/kuka_tcp_client.h>
#include <caros_kuka_lwr/KukaWriteRawService.h>

#include <rw/common/Ptr.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/math_fwd.hpp>

#include <queue>
#include <string>
#include <vector>

#define SUPPORTED_Q_LENGTH_FOR_KUKA 7

namespace rw
{
namespace models
{
class WorkCell;
class Device;
}  // namespace models
}  // namespace rw

namespace rw
{
namespace invkin
{
class JacobianIKSolver;
}
}  // namespace rw

namespace caros
{
class KukaRobots : public caros::CarosNodeServiceInterface, public caros::SerialDeviceServiceInterface
{
 public:
  explicit KukaRobots(const ros::NodeHandle& nodehandle);

  virtual ~KukaRobots();

  enum KUKANODE_ERRORCODE
  {
    KUKANODE_MISSING_PARAMETER = 1,
    KUKANODE_SERVICE_CONFIGURE_FAIL,
    KUKANODE_SERIALDEVICESERVICE_CONFIGURE_FAIL,
    KUKANODE_MISSING_WORKCELL,
    KUKANODE_NO_SUCH_DEVICE_IN_WORKCELL,
    KUKANODE_INVALID_CALLBACKPORT,
    KUKANODE_UNSUCCESSFUL_CONNECT,
    KUKANODE_UNSUPPORTED_Q_LENGTH,
    KUKANODE_INTERNAL_ERROR
  };

  enum KUKA_LWR_FEEDBACK_STATES
  {
    LWR_HELLO = 0,
    LWR_TOOL_FRAME = 1,
    LWR_FLANGE_FRAME = 2,
    LWR_JOINT_POS = 4,
    LWR_STATUS = 8,
    LWR_CART_FORCE = 16,
    LWR_JOINT_TORQUE = 32
  };

  /************************************************************************
   * SerialDeviceServiceInterface functions
   ************************************************************************/
  //! @copydoc caros::SerialDeviceServiceInterface::moveLin
  bool moveLin(const TransformAndSpeedAndBlendContainer_t& targets);
  //! @copydoc caros::SerialDeviceServiceInterface::movePtp
  bool movePtp(const QAndSpeedAndBlendContainer_t& targets);
  //! @copydoc caros::SerialDeviceServiceInterface::movePtpT
  bool movePtpT(const TransformAndSpeedAndBlendContainer_t& targets);
  //! @copydoc caros::SerialDeviceServiceInterface::moveVelQ
  bool moveVelQ(const rw::math::Q& q_vel);
  //! @copydoc caros::SerialDeviceServiceInterface::moveVelT
  bool moveVelT(const rw::math::VelocityScrew6D<>& t_vel);
  //! @copydoc caros::SerialDeviceServiceInterface::moveServoQ
  bool moveServoQ(const QAndSpeedContainer_t& targets);
  //! @copydoc caros::SerialDeviceServiceInterface::moveServoT
  bool moveServoT(const TransformAndSpeedContainer_t& targets);
  //! @copydoc caros::SerialDeviceServiceInterface::moveStop
  bool moveStop();

 protected:
  /************************************************************************
   * Hooks implemented from CarosNodeServiceInterface base class
   ************************************************************************/
  bool activateHook();
  bool recoverHook(const std::string& error_msg, const int64_t error_code);

  void runLoopHook();
  void errorLoopHook();
  void fatalErrorLoopHook();

  /************************************************************************
   * Services special to kuka
   ************************************************************************/
  bool KukaWriteRaw(caros_kuka_lwr::KukaWriteRawService::Request& request,
                    caros_kuka_lwr::KukaWriteRawService::Response& response);

 private:
  /* convenience functions */
  bool isInWorkingCondition();
  bool supportedQSize(const rw::math::Q& q);
  void set_robot_feedbacks(char states);
  void write(std::string msg);

 private:
  ros::NodeHandle nodehandle_;
  rw::math::Q qcurrent_; /* Updated in runLoopHook() */

  rw::common::Ptr<kuka_lwr> current_robot_state_;
  boost::asio::io_service io_service_;
  std::thread tcp_thread_;
  rw::common::Ptr<tcp_client> client_sock_;
  ros::ServiceServer srv_raw_message_;
};  // end of class KukaRobots

void tokenize(const std::string& input, std::vector<std::string>& output, char token = ' ');
}  // namespace caros

#endif  // CAROS_KUKA_ROBOTS_H
